package controlSoftware.userThread;


public class SensorData{
	
	// IMU measurement
	public double accel_x;
	public double accel_y;
	public double accel_z;
	public double pitch;
	public double roll;
	public double yaw;
	
	// Calculated velocities
	public Velocity currVel;
	
	
	// Calculated displacements
	public double disp_x;
	public double disp_y;
	
	// Coordinates for rendering
	public Coordinate currLoc;
	
	Device target;
	
	public SensorData(double x, double y, double z, double pitch, double roll, double yaw, Device testObj){
		this.accel_x = x;
		this.accel_y = y;
		this.accel_z = z;
		this.pitch = pitch;
		this.roll = roll;
		this.yaw = yaw;
		this.target = testObj;
		
		currVel = new Velocity(0,0);
		currLoc = new Coordinate (0,0);
	}
	
	public int processRaw()
	{
		// converts IMU readings to acceleration values
		
		//TODO: calculate sens and bias for x and y
		
		//a = (x [V] -bias[V])/sens[V/g] = [g]=9.8m/ss
		
		accel_x = (accel_x - SysModel.ax_bias) / SysModel.ax_sens * SysModel.g;
		accel_y = (accel_y - SysModel.ay_bias) / SysModel.ay_sens * SysModel.g;
		
		System.out.println("processRaw => " + accel_x + " , " + accel_y);
		
		return 0;
	}
	
	public Velocity getVelocity(Velocity prevV)
	{
		
		if (currVel!= null){
			currVel.x = currVel.calcNewVelocity(prevV.x, accel_x, SysModel.sampleTime);
			currVel.y = currVel.calcNewVelocity(prevV.y, accel_y, SysModel.sampleTime);
			
			return currVel;
		}		
		else
		{
			System.out.println("getVelocity: Null Velocity");
		}
		return currVel;
	}

	public int getDisplacement() 
	{
		disp_x = currVel.x * SysModel.sampleTime/1000;
		disp_y = currVel.y * SysModel.sampleTime/1000;
		
		System.out.println("getDisp - " + disp_x);
		return 1;	
	}
	
	public Coordinate getCoordinate(Coordinate prev)
	{
		
		// disp [m] * ratio [pix/m] = x [pix]
		currLoc.x = prev.x + (disp_x * SysModel.mapRatio_x);
		currLoc.y = prev.y + (disp_y * SysModel.mapRatio_y);
		
		return currLoc;
	}
}